#include "mainwindow.h"

#include "ui_mainwindow.h"

MainWindow::MainWindow( int argc, char** argv, QWidget* parent ) : QMainWindow( parent ), argc( argc ), argv( argv ), ui( new Ui::MainWindow ) {
    ui->setupUi( this );

    // TODO qnode slam导航初始化
    qnode_module = new qnode_new::QNodeNew( argc, argv );
    qnode_module->start();
}

MainWindow::~MainWindow() {
    delete ui;
}

void MainWindow::on_force_slow_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "std_srvs::SetBool" );
    qjob_0.insert( "info", "强制减速" );
    qjob_0.insert( "data", true );
    QJsonObject qobj;
    commonServiceBack( "force_show", qnode_module->slotCommonService( commonServiceJson( "force_show", qjob_0 ) ), qobj );
    if ( qobj.contains( "value" ) && qobj.value( "value" ).toBool( false ) ) { qDebug() << "强制减速设定成功:" << qobj.value( "info" ).toString( "不存在info!" ); }
    else {
        qDebug() << "强制减速设定失败!" << qobj.value( "info" ).toString( "不存在info!" );
    }
}

void MainWindow::on_can_initial_pose_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "std_srvs::SetBool" );
    qjob_0.insert( "info", "锁定AMCL重定位" );
    qjob_0.insert( "data", true );
    QJsonObject qobj;
    commonServiceBack( "/can_initial_pose", qnode_module->slotCommonService( commonServiceJson( "/can_initial_pose", qjob_0 ) ), qobj );
    if ( qobj.contains( "value" ) && qobj.value( "value" ).toBool( false ) ) { qDebug() << "锁定AMCL重定位设定成功:" << qobj.value( "info" ).toString( "不存在info!" ); }
    else {
        qDebug() << "锁定AMCL重定位失败!" << qobj.value( "info" ).toString( "不存在info!" );
    }
}

void MainWindow::on_reflective_localization_enable_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "std_srvs::SetBool" );
    qjob_0.insert( "info", "使能反光板" );
    qjob_0.insert( "data", true );
    QJsonObject qobj;
    commonServiceBack( "/reflective_localization_enable", qnode_module->slotCommonService( commonServiceJson( "/reflective_localization_enable", qjob_0 ) ), qobj );
    if ( qobj.contains( "value" ) && qobj.value( "value" ).toBool( false ) ) { qDebug() << "使能反光板设定成功:" << qobj.value( "info" ).toString( "不存在info!" ); }
    else {
        qDebug() << "使能反光板失败!" << qobj.value( "info" ).toString( "不存在info!" );
    }
}

void MainWindow::on_reflective_localization_reset_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "ros_qt_msgs::RefReset" );
    qjob_0.insert( "info", "使能反光板,带全局位置参数" );
    qjob_0.insert( "use_global_position", true );
    qjob_0.insert( "x", 100 );
    qjob_0.insert( "y", 100 );
    QJsonObject qobj;
    commonServiceBack( "/reflective_localization_reset", qnode_module->slotCommonService( commonServiceJson( "/reflective_localization_reset", qjob_0 ) ), qobj );
    if ( qobj.contains( "value" ) && qobj.value( "value" ).toBool( false ) ) { qDebug() << "使能反光板,带全局位置参数成功:" << qobj.value( "info" ).toString( "不存在info!" ); }
    else {
        qDebug() << "使能反光板,带全局位置参数!" << qobj.value( "info" ).toString( "不存在info!" );
    }
}

QByteArray MainWindow::commonServiceJson( QString name, const QJsonObject& json ) {
    QJsonArray qarray;
    qarray.insert( 0, json );
    return commonServiceJsons( name, qarray );
}

QByteArray MainWindow::commonServiceJsons( QString name, const QJsonArray& jsons ) {
    QJsonObject qjob;
    qjob.insert( "name", name );
    qjob.insert( "values", jsons );
    QJsonDocument qdcm( qjob );
    return qdcm.toJson();
}

void MainWindow::commonServiceBack( QString name, const QByteArray& qbyteArray, QJsonObject& qobj ) {
    QJsonParseError qjerror;
    QJsonDocument   qjd   = QJsonDocument::fromJson( qbyteArray, &qjerror );
    QJsonObject     bqobj = qjd.object();
    qDebug() << "bqobj" << bqobj;
    if ( bqobj.contains( "name" ) && bqobj.value( "name" ) == name ) {
        if ( bqobj.contains( "result" ) ) {
            if ( bqobj.value( "result" ) == "success" ) {
                if ( bqobj.contains( "data" ) ) {
                    QJsonArray qjarray = bqobj.value( "data" ).toArray();
                    qobj               = qjarray.takeAt( 0 ).toObject();
                }
                else {
                    qobj.insert( "info", "无返回数据" );
                }
            }
            else {
                qobj.insert( "info", "服务异常" + bqobj.value( "info" ).toString( "不存在info!" ) );
            }
        }
    }
    else {
        qobj.insert( "info", "服务名称异常" );
    }
}

void MainWindow::on_initialpose_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "geometry_msgs::PoseWithCovarianceStamped" );
    qjob_0.insert( "info", "amcl指定定位范围内定位" );
    qjob_0.insert( "orientation", 1.5 );
    qjob_0.insert( "x", 100 );
    qjob_0.insert( "y", 100 );
    qnode_module->slotCommonPblish( commonServiceJson( "/initialpose", qjob_0 ) );
}

void MainWindow::on_startinitpose_clicked() {
    QJsonObject qjob_0;
    qjob_0.insert( "ros_type", "std_msgs::Bool" );
    qjob_0.insert( "info", "开启自动重定位" );
    qjob_0.insert( "data", true );
    qnode_module->slotCommonPblish( commonServiceJson( "/startinitpose", qjob_0 ) );
}

void MainWindow::on_navgoal_clicked() {}

void MainWindow::on_pushButton_clicked() {
    qDebug() << "激光导航";
    QJsonObject qjob_0;
    QJsonObject point;
    point.insert( "x", 1 );
    point.insert( "y", 1 );
    point.insert( "id", 1 );

    qjob_0.insert( "start", point );
    QJsonArray targets;
    targets.append( point );
    targets.append( point );
    targets.append( point );
    targets.append( point );
    targets.append( point );
    targets.append( point );
    targets.append( point );
    targets.append( point );
    qjob_0.insert( "targets", targets );
    QJsonArray  speeds;
    QJsonObject speed1;
    speed1.insert( "dist", 10.0 );
    speed1.insert( "speed", 1 );
    speeds.append( speed1 );
    speeds.append( speed1 );
    speeds.append( speed1 );
    speeds.append( speed1 );
    qjob_0.insert( "speeds", speeds );
    qjob_0.insert( "motion_dir", 1 );
    QJsonObject cp_asse;
    cp_asse.insert( "x", 1 );
    cp_asse.insert( "y", 1 );
    cp_asse.insert( "yaw", 1 );
    qjob_0.insert( "cp_asse", cp_asse );
    qjob_0.insert( "cp_tg", cp_asse );
    QJsonDocument qdcm( qjob_0 );
    qnode_module->slotCommonMoveBaseAction( 1, qdcm.toJson() );
}
/**
 * @brief MainWindow::on_pushButton_2_clicked
 */
void MainWindow::on_pushButton_2_clicked() {
    qDebug() << "取消导航";
    qnode_module->slotCommonMoveBaseAction( -1, "" );
}
